package clubhouse.antislam;

import java.text.DecimalFormat;
import java.util.ListIterator;
import java.util.Vector;

import clubhouse.base.Base;
import clubhouse.control.DataChecker;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class AntiSLAM {
	
	/**The current scenario.*/
	private int scenario;
	/**The enabling Comparing Anti-SLAM state, true if simulating the comparing Anti-SLAM simulation, 
	 * false otherwise.*/ 
	private boolean enabled;
	
	//Scenario 2&3
	/**The set of real landmarks, as generated.*/
	private Vector<AntiSlamNode> realNodes;
	/********Anti-SLAM strategy constants********/
	/**Translation Anti-SLAM constant.*/
	public static final int TRANSLATION = 1;
	/**Rotation Anti-SLAM constant.*/
	public static final int ROTATION = 2;
	/**Hybrid Anti-SLAM constant.*/
	public static final int HYBRID = 3;
	/********************************************/

	/**Initial value constant.*/
	public static final int INITVALUE = -1;
	/**Node deviation value for which the Anti-SLAM process is stopped.*/
	public static final int STOP = -2;
	/**Factor value distance robot/node above which the node is disable.*/
	private static final double DISABLE_N0DE_FACTOR = 1;
	/**Value of constant deviation in Rotation Anti-SLAM strategy*/
	private static final double ASROTATION_DEVIATION = -Math.PI/20;
	
	/**The value of current Anti-SLAM strategy, equals to one of the Anti-SLAM strategy constants.*/
	private int antiSlamType;

	/**Used only for Hybrid strategy, robot position at the previous step.*/
	private double[] previousRobot;
	/**Used only for Hybrid strategy, the current trap Node, node to which the Anti-SLAM try to bring the robot.*/
	private AntiSlamNode trapNode;
	/**Used to disable Anti-SLAM if not working after too many steps.*/
	private int step;
	/**Value of step where Anti-SLAM is enable above which, it is considered as inefficient and stopped.*/
	private static final int MAX_AS_STEP = 300;
	/**Used only for Hybrid strategy, current value of node on which Anti-SLAM error has been applied at the 
	 * current step.*/
	private int nodeDone = 0;
	
	/**
	 * Constructs the AntiSLAM object according to the specified scenario, generated landmarks and Anti-SLAM 
	 * strategy.
	 * @param scenario the current scenario.
	 * @param nodes the set of real landmarks (as generated).
	 * @param ASType the current type of Anti-SLAM, equals to one of the Anti-SLAM strategy constants.
	 */
	public AntiSLAM(int scenario, Vector<double[]> nodes, int ASType) {
		this.scenario = scenario;
		this.enabled = false;
		
		if (this.scenario!=1){
			this.previousRobot = null;
			this.trapNode = null;
			this.nodeDone = 0;
			this.step = 0;
			double[] tempAddNode;
			AntiSlamNode tempSlamNode;
			if (nodes != null) {
				this.realNodes = new Vector<AntiSlamNode>(nodes.size(),1);
				ListIterator<double[]> li = nodes.listIterator();
				while(li.hasNext()){
					tempAddNode = li.next();
					tempSlamNode = new AntiSlamNode(tempAddNode);
					this.realNodes.add(tempSlamNode);
				}
			} else {
				this.realNodes = new Vector<AntiSlamNode>(5,1);
			}
	        this.antiSlamType = ASType;
		}
	}
	
	/**
	 * Get the AntiSlamNode in the current set of Anti-SLAM node, having an id equals to the specified node id.
	 * @param nodeId the id of the Anti-SLAM to get.
	 * @return the AntiSlamNode object with the id than the specified node id.
	 */
	private AntiSlamNode getRealNodeFromId(int nodeId){
		AntiSlamNode tempNode;
		ListIterator<AntiSlamNode> li = this.realNodes.listIterator();
        while(li.hasNext()){
        	tempNode = li.next();
        	if (tempNode.getNodeId()==nodeId) return(tempNode);
        }
        return(null);
	}
	
	/**
	 * Update real landmarks from specified landmarks, add a real landmark only if it wasn't known before.
	 * @param realLandmarks the new real landmarks as generated or as received before processing them.
	 */
	public void setRealLandmarks(Vector<double[]> realLandmarks) {
		double[] tempAddNode;
		AntiSlamNode tempSlamNode;        
		ListIterator<double[]> li = realLandmarks.listIterator();
        while(li.hasNext()){
        	tempAddNode = li.next();
        	if (this.getRealNodeFromId((int)tempAddNode[0])==null){
            	tempSlamNode = new AntiSlamNode(tempAddNode);
            	this.realNodes.add(tempSlamNode);
        	}
        }
	}

	/**
	 * Calculate the number of landmark in which the robot is in their communication range.
	 * @param x_robot the X coordinate of the current robot position.
	 * @param y_robot the Y coordinate of the current robot position.
	 * @return the number of node in which the robot is their detection range.
	 */
	private int nbCloseNodes(double x_robot, double y_robot){
		AntiSlamNode tempLandmark = null;
		int nbCloseLandmarks = 0;
		
		ListIterator<AntiSlamNode> li = this.realNodes.listIterator();
	    while(li.hasNext()){
	        tempLandmark = li.next();
	        if (Base.euclideanDist(tempLandmark.getX(), tempLandmark.getY(), x_robot, y_robot)<=AntiSLAM.DISABLE_N0DE_FACTOR*DataChecker.communicationRange){
	        	nbCloseLandmarks++;
	        }
	    }
	    if (Base.DEBUG) System.out.println("(nbCloseNodes) nbCloseNodes = "+nbCloseLandmarks);
	    return nbCloseLandmarks;
	}
	
	/**
	 * @param x_robot
	 * @param y_robot
	 * @return
	 */
	private boolean isRobotSensed(double x_robot, double y_robot){
		AntiSlamNode tempLandmark = null;
		
		ListIterator<AntiSlamNode> li = this.realNodes.listIterator();
	    while(li.hasNext()){
	        tempLandmark = li.next();
	        if (Base.euclideanDist(tempLandmark.getX(), tempLandmark.getY(), x_robot, y_robot)<=DataChecker.sensingRange){
	        	if (Base.DEBUG) System.out.println("(isSensed) Robot sensed by node"+tempLandmark.getNodeId());
	        	return true;
	        }
	    }
	    return false;
	}
	
	/**
	 * Apply Anti-SLAM error on coordinates of the specified node for the specified robot position.
	 * @param nodeId the id of the node on which the Anti-SLAM is applied.
	 * @param initCoords the coordinates of the node on which the Anti-SLAM is applied [X coordinate, Y coordinate].
	 * @param x_robot the X coordinate of the current robot position.
	 * @param y_robot the Y coordinate of the current robot position.
	 * @return the node's coordinates with Anti-SLAM error applied.
	 */
	public double[] applyAntiSlam(int nodeId, double[] initCoords, double x_robot, double y_robot)  {
		double [] result = null; 
		DecimalFormat df = new DecimalFormat("#####.##"); 
		
		if (this.scenario==1){
			if (!this.enabled){
				if (x_robot<0) result = initCoords;
				else this.enabled = true;
			} 
			if (this.enabled){
				switch (nodeId){
				case 1:		//Node 1, (0,20) then (10,15)
					result = new double[]{10,15};
					break;
				case 2:		//Node 2, (10,15)
					result = new double[]{10,15};
					break;
				case 3:		//Node 3, (15,5)
					result = new double[]{15,5};
					break;
				case 4:		//Node 4, (5,5)
					result = new double[]{5,5};
					break;
				case 5:		//Node 5, do nothing
					result = initCoords;
					break;
				default:
					result = initCoords;
					break;
				}
			}
		} else {		//Scenario 2 & 3
			/******Manage Anti-SLAM status******/
			if ((this.isRobotSensed(x_robot,y_robot))&&(this.step<MAX_AS_STEP)) this.enabled = true;
			else this.enabled = false;
			if ((this.trapNode != null)&&(Base.euclideanDist(this.trapNode.getX(), this.trapNode.getY(), x_robot, y_robot)<=DataChecker.actingRange)){
				this.enabled = false;
			}
			/***********************************/
			if (this.enabled){	//Anti-SLAM enable
				AntiSlamNode currentNode = this.getRealNodeFromId(nodeId);

				double dist = Base.euclideanDist(currentNode.getX(), currentNode.getY(), x_robot, y_robot);
				double angle = Math.atan((currentNode.getY()-y_robot)/(currentNode.getX()-x_robot));
				if (currentNode.getX()<x_robot) angle=-angle;	//deal with atan's property
				
				double[] antiSlamedCoord = new double[2];
				double currentDeviation = currentNode.getDeviation();
				
				/******Manage node status******/
				if (currentDeviation == AntiSLAM.STOP) antiSlamedCoord = initCoords;
				else if (dist>AntiSLAM.DISABLE_N0DE_FACTOR*DataChecker.communicationRange){
					if (currentDeviation != AntiSLAM.INITVALUE) {
						currentNode.setDeviation(AntiSLAM.INITVALUE);
						if (Base.DEBUG) System.out.println("(applyAntiSlam) Node "+df.format(currentNode.getNodeId())+" Stopped");
					}
					antiSlamedCoord =  initCoords;		
				/*******************************/
				} else {	//Node is running
					if (currentNode.getDeviation() == AntiSLAM.INITVALUE){
						currentNode.setDist(dist);
						currentNode.setAngle(angle);
					}
					dist = currentNode.getDist();
					angle = currentNode.getAngle();

					switch (antiSlamType){
						case AntiSLAM.TRANSLATION:		//1st algorithm : translation
							currentNode.setDeviation(0);
							antiSlamedCoord[0] = dist * Math.cos(angle)+x_robot;
							antiSlamedCoord[1] = dist * Math.sin(angle)+y_robot;
							break;
						case AntiSLAM.ROTATION:			//2nd algorithm : rotation
							currentNode.setDeviation(0);
							antiSlamedCoord[0] = dist * Math.cos(angle+AntiSLAM.ASROTATION_DEVIATION)+x_robot;
							antiSlamedCoord[1] = dist * Math.sin(angle+AntiSLAM.ASROTATION_DEVIATION)+y_robot;
							break;
						case AntiSLAM.HYBRID:			//3rd algorithm, hybrid
							if ((this.previousRobot==null)){	//Previous robot position unknown
								this.previousRobot = new double[]{x_robot,y_robot};
								antiSlamedCoord = initCoords;
							} else if (Base.euclideanDist(x_robot,y_robot, this.previousRobot[0],this.previousRobot[1])==0) {
								antiSlamedCoord = initCoords;	//Robot didn't move
							} else {	//Previous robot position ok, do Anti-SLAM
	
								//Get current robot direction
								double r_robot = Math.atan((y_robot-this.previousRobot[1])/(x_robot-this.previousRobot[0]));
								if (x_robot<this.previousRobot[0]) r_robot -= Math.PI;	//Deal with atan property
								
								/*******Choose trap node*******/
								int indexTrapNode;
								//Test if a trap node exists
								int nbTrapNodes = 0;
								for (int i=0;i<this.realNodes.size();i++){
									if ((this.realNodes.get(i).getX()>=x_robot)&&(Base.euclideanDist(this.realNodes.get(i).getX(), this.realNodes.get(i).getY(), x_robot, y_robot)<=DataChecker.communicationRange)) nbTrapNodes++;
								}
								if (nbTrapNodes == 0) {	//No possible trap node
									antiSlamedCoord = initCoords;
								} else {
									//Choose randomly a trap node
									while ((this.trapNode == null)||(this.trapNode.getX()<x_robot)||(Base.euclideanDist(this.trapNode.getX(), this.trapNode.getY(), x_robot, y_robot)>DataChecker.communicationRange)){
										indexTrapNode = (int) Math.round(Math.random()*(this.realNodes.size()-1)+1);
										this.trapNode = this.getRealNodeFromId(indexTrapNode);
									}
								/******************************/	
									//Get its direction according to the robot's one
									double trapNodeAngle = Math.atan((this.trapNode.getY()-y_robot)/(this.trapNode.getX()-x_robot));
									
									//Get current node Angle
									double currentNodeAngle = Math.atan((currentNode.getY()-y_robot)/(currentNode.getX()-x_robot));
									if (currentNode.getX()<x_robot) currentNodeAngle -= Math.PI; 	//Deal with atan property
									
									//Calculate & store deviation
									double deviation = 0;
									deviation =  trapNodeAngle - r_robot;
									currentNode.setDeviation(deviation);
									
									//Calculate Anti-SLAMed coordinates
									antiSlamedCoord[0] = dist * Math.cos(currentNodeAngle+currentNode.getDeviation())+x_robot;
									antiSlamedCoord[1] = dist * Math.sin(currentNodeAngle+currentNode.getDeviation())+y_robot;

									if (Base.DEBUG) System.out.println("(applyAntiSlam) (Node "+df.format(currentNode.getNodeId())+"), Trap node is node "+df.format(this.trapNode.getNodeId())+" ("+this.trapNode.getX()+","+this.trapNode.getY()+")"+", r_robot = "+df.format(r_robot)+", trapNode Angle = "+df.format(trapNodeAngle)+", deviation = "+df.format(deviation)+", CurrentNodeAngle = "+currentNodeAngle+", finalAngle = CurrentNodeAngle + deviation = "+df.format(currentNodeAngle+currentNode.getDeviation()));
								}
							}	
							/***Manage previous robot position***/
							this.nodeDone++;
							if (this.nodeDone == this.nbCloseNodes(x_robot,y_robot)){
								this.previousRobot[0] = x_robot;
								this.previousRobot[1] = y_robot;
								this.nodeDone = 0;
								this.step++;
							}
							/************************************/
							break;
						default:
							break;
					}
				}	
				if (Base.DEBUG){
					double finalDist = Base.euclideanDist(antiSlamedCoord[0], antiSlamedCoord[1], x_robot, y_robot);
					double finalAngle = Math.atan((antiSlamedCoord[1]-y_robot)/(antiSlamedCoord[0]-x_robot));
					if (antiSlamedCoord[0]<x_robot) finalAngle -= Math.PI;	//Deal with atan property
					System.out.println("(applyAntiSlam) Node "+nodeId+", initDist = "+dist+", finalDist = "+finalDist+", initAngle = "+angle+", finalAngle = "+finalAngle);
				}
				result = antiSlamedCoord;			
			} else {
				result = initCoords;
			}
		}
		return result;
	}

}
